/* Example program for moving a Servo.

  TODO: next step - use ADC from resistor to control the servo
*/
#include <avr/io.h>
#include <avr/interrupt.h>

#ifndef F_CPU
#warning "F_CPU was not defined yet, now using 3686400 Hz"
#define F_CPU 3686400UL     /* 3.6864 Mhz */
#endif
#include <util/delay.h>

uint16_t g_iLastDelay=0;

void init(void)
{
  // output only on PortB
  DDRB  = 0x3F;
  // turn all LEDs off
  PORTB = 0x3F;
}

void setPosition(int16_t p_iDelay)
{
  PORTB=PINB | 0x01;
  _delay_us(400);
  _delay_us(p_iDelay);
  PORTB=PINB & ~0x01;

  // wait for 20 ms
  _delay_us(1600-p_iDelay);
  _delay_ms(18);
}

void pushPosition(int16_t p_iDelay)
{
  setPosition(p_iDelay);
  g_iLastDelay=p_iDelay;
}

int main (void)
{
  enum STATE_MAIN { eSTATE_INIT, eSTATE_FORWARD, eSTATE_BACKWARDS};
  enum STATE_MAIN state=eSTATE_INIT;

  int16_t timer=0;
  int16_t direction=10;

  init();  
  sei(); // interrupts ON

  // now loop forever
  while(1)
  {
    switch(state)
    {
      case eSTATE_INIT:
        state=eSTATE_FORWARD;
        break;
      case eSTATE_FORWARD:
        pushPosition(0);
        // next step for the timer
        timer=timer+direction;

        if (timer+direction > 1000)
        {
          PORTB=PINB | 0x02;
          state=eSTATE_BACKWARDS;
        }
        break;
      case eSTATE_BACKWARDS:
        pushPosition(1600);
        // next step for the timer
        timer=timer-direction;

        if (timer < direction)
        {
          state=eSTATE_FORWARD;
          PORTB=PINB & ~0x02;
        }
        break;
      default:
        state=eSTATE_FORWARD;
        timer=0;
        break;
    }
  }

  // pointless, will never be reached
  return 0;
}
